#include <math.h>
#include <stdlib.h>
#include "fdecl.h"
#include "misc.h"


double *sinu;	/* precomputed sin(u) array */
double *cosu;	/* precomputed cos(u) array */
double *sinv;	/* precomputed sin(v) array */
double *cosv;	/* precomputed cos(v) array */

double u_offset, v_offset, u_scale, v_scale;


void allocate_sin_cos_arrays(int usize, int vsize)
{
  sinu = (double *) calloc(usize + 2, sizeof(double));
  cosu = (double *) calloc(usize + 2, sizeof(double));
  sinv = (double *) calloc(vsize, sizeof(double));
  cosv = (double *) calloc(vsize, sizeof(double));
}


void compute_sin_cos_arrays(int usize, int vsize)
{
  int i;
  double u, v, ustep, vstep, pi = 3.14159265358979323; /* fix me */

  ustep = pi / (usize + 1); vstep = 2.0 * pi / vsize;

  u_offset = pi/2.0; v_offset = pi; /* global scale factors for now */
  u_scale = 1.0 / ustep; v_scale = 1.0 / vstep;

  for (u = -pi/2.0 + ustep, i = 1; i < usize + 1; u += ustep, i++) {
    sinu[i] = sin(u); cosu[i] = cos(u);
  }

  sinu[0] = -1.0; cosu[0] = 0.0; /* s pole */
  sinu[usize+1] = 1.0; cosu[usize+1] = 0.0; /* n pole */

  for (v = -pi, i = 0; i < vsize; v += vstep, i++) {
    sinv[i] = sin(v); cosv[i] = cos(v);
  }
}


void compute_superquadric(hybroid *h)
{
  double r1 = h->sc * h->a1, r2 = h->sc * h->a2, r3 = h->sc * h->a3;
  double ee1 = h->e1, ee2 = h->e2;
  int i;
  node *nds = h->nodes;

  for (i = 0; i < h->numnodes; i++) 
    compute_superquadric_node(&nds[i], r1, r2, r3, ee1, ee2);

}


void compute_superquadric_node(node *nde, 
			       double r1, double r2, double r3,
			       double e1, double e2)
{
  int u = (int) ((nde->uu + u_offset) * u_scale + 0.5); 
  int v = (int) ((nde->vv + v_offset) * v_scale + 0.5);
  double suu = sinu[u], cuu = cosu[u], cvv = cosv[v], svv = sinv[v];
  vector *r = &nde->refer;

  r->x = r1 * Sign(cuu) * pow(fabs(cuu), e1) * Sign(cvv) * pow(fabs(cvv), e2);
  r->y = r2 * Sign(cuu) * pow(fabs(cuu), e1) * Sign(svv) * pow(fabs(svv), e2);
  r->z = r3 * Sign(suu) * pow(fabs(suu), e1);
}


void compute_superquadric_taper_bend(hybroid *h)
{
  double r1 = h->sc * h->a1, r2 = h->sc * h->a2, r3 = h->sc * h->a3;
  double ee1 = h->e1, ee2 = h->e2;
  int i;
  node *nds = h->nodes;

  for (i = 0; i < h->numnodes; i++) 
    compute_superquadric_node_taper_bend(h, &nds[i], r1, r2, r3, ee1, ee2);

}


void compute_superquadric_node_taper_bend(hybroid *h, node *nde, 
			       double r1, double r2, double r3,
			       double e1, double e2)
{
  double cuu = cos(nde->uu), cvv = cos(nde->vv);
  double suu = sin(nde->uu), svv = sin(nde->vv);

  vector *r = &nde->refer;
  double pi = 3.14159265358979323;

/* Another slower way of computing superquadric parameters */
/*
  int u = (int) rint((nde->uu + u_offset) * u_scale); 
  int v = (int) rint((nde->vv + v_offset) * v_scale);
  double suu = sinu[u], cuu = cosu[u], cvv = cosv[v], svv = sinv[v];
*/
  
/*unused parameters for twisting and wrong tapering */
/*
  double twist1 = 1.0/3.0;
  double r11, r22, bet, curv = 2.0, alph = 2.0;
*/

/*
  r->x = r1 * Sign(cuu) * pow(fabs(cuu), e1) * Sign(cvv) * pow(fabs(cvv), e2);
  r->y = r2 * Sign(cuu) * pow(fabs(cuu), e1) * Sign(svv) * pow(fabs(svv), e2);
  r->z = r3 * Sign(suu) * pow(fabs(suu), e1);
*/
  r->x = r1 * Sign(cuu) * pow(fabs(cuu), e1) * Sign(cvv) * pow(fabs(cvv), e2);
  r->y = r2 * Sign(cuu) * pow(fabs(cuu), e1) * Sign(svv) * pow(fabs(svv), e2);
  r->z = r3 * Sign(suu) * pow(fabs(suu), e1);

/* TAPERING */
/* the bigger tapx and tapy are the one end becomes sharper */
/* r3 = sc * a3 */

  r->x = (h->tapx * r->z / r3 + 1) * r->x;
  r->y = (h->tapy * r->z / r3 + 1) * r->y;

/* BENDING */
/* pi * bend3 conrtrols the region that the bending is applied */
/* bend3 should be equal to 1.0, 1.0/2.0, 1.0/3.0 etc */
/* bend1 controls the amount of bending */
/* bend2 defines the location of the bending in the z-direction */
/* Bending is in the x direction, same formula for y direction */
/*
  r->y = r->y + h->bend1 * cos((r->z + h->bend2) / r3 * pi * h->bend3);
*/
  r->x = r->x + h->bend1 * cos((r->z + h->bend2) / r3 * pi * h->bend3);




/* TWISTING not used in parameter recovery*/
/* one parameter i.e twist1 */
/*
  r11 = r->z / r3 * pi * twist1;

  r->x = r->x * cos(r11) - r->y * sin(r11);
  r->y = r->x * sin(r11) + r->y * cos(r11);
*/


/* WRONG BENDING */
/* 0.3 = length in x direction */
/* bending */
/*
  r->x = r->x + 0.8;
  bet = atan(r->y/r->x);
  r11 = cos(alph - bet) * pow(r->x * r->x + r->y * r->y, 1.0/2.0);
  r22 = 1.0/curv - cos(r->z * 1.0/curv) * (1.0/curv - r11);

  r->x = r->x + cos(alph) * (r22 - r11);
  r->y = r->y + sin(alph) * (r22 - r11);
  r->z = sin(r->z * 1.0/curv) * (1.0/curv - r11);
*/
/* END OF WRONG BENDING */
}
